#include "camera.h"

// #include "image_decoder.h"
int camera_fd_;
int ret;

CAMERA::CAMERA(uint32_t width, uint32_t height) : width_(width),height_(height) {

}

CAMERA::~CAMERA(){

    camera_close();
}

bool CAMERA::camera_init(uint32_t width, uint32_t height){
    // 1. 打开设备
    char path[30];
    for(int i = 0 ; i < 10; i++){
        snprintf(path,sizeof(path),"/dev/video%d",i);
        camera_fd_ = open(path, O_RDWR);
        if (camera_fd_ > 0) {
            std::cout << "打开设备成功" <<std::endl;
            break;
        } 
        std::cout << "打开设备失败" <<std::endl;
        if (camera_fd_ > 0) {
            close(camera_fd_);
            camera_fd_ = -1;
        }
    }

    // 2. 设置摄像头格式
    struct v4l2_format v4l2_fmt_set;
    memset(&v4l2_fmt_set, 0, sizeof(v4l2_fmt_set));
    v4l2_fmt_set.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    v4l2_fmt_set.fmt.pix.width = width;
    v4l2_fmt_set.fmt.pix.height = height;
    v4l2_fmt_set.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    v4l2_fmt_set.fmt.pix.sizeimage = width * height * 2;
    // v4l2_fmt_set.fmt.pix.field = V4L2_FIELD_NONE;

    if (ioctl(camera_fd_, VIDIOC_S_FMT, &v4l2_fmt_set) < 0) {
        perror("设置格式失败");
        return false;
    }


    // 3. 申请缓冲区队列
    struct v4l2_requestbuffers req_buf;
    memset(&req_buf, 0, sizeof(req_buf));
    req_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req_buf.count = max_buffer;
    req_buf.memory = V4L2_MEMORY_MMAP;

    if (ioctl(camera_fd_, VIDIOC_REQBUFS, &req_buf) < 0) {
        printf("hello");
        perror("申请队列空间失败");
        return false;

    }

    // printf("实际分配的缓冲区数量: %d\n", req_buf.count);
    // 4. 内存映射
    for (int i = 0; i < req_buf.count; i++) {
        imx_mapbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        imx_mapbuffer.memory = V4L2_MEMORY_MMAP;
        imx_mapbuffer.index = i;
        imx_mapbuffer.m.offset = 0;

        if (ioctl(camera_fd_, VIDIOC_QUERYBUF, &imx_mapbuffer) < 0) {
            perror("查询缓冲区失败");
            continue;
        }


        imx_buf_length[i] = imx_mapbuffer.length;
        imx_mpaddr[i] = (unsigned char *)mmap(NULL, imx_mapbuffer.length,
                                            PROT_READ | PROT_WRITE, MAP_SHARED,
                                            camera_fd_, imx_mapbuffer.m.offset);
        
        if (imx_mpaddr[i] == MAP_FAILED) {
            perror("映射失败");
            continue;
        }

        if (ioctl(camera_fd_, VIDIOC_QBUF, &imx_mapbuffer) < 0) {
            perror("缓冲区入队失败");
            return false;
        }
    }

    return true;
}

void CAMERA::camera_on(){
    // 5. 开始采集
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(camera_fd_, VIDIOC_STREAMON, &type) < 0) {
        perror("开启采集失败");
        exit(1);
    }
}


uint8_t * CAMERA::camera_take_one(int *byte_used) {
    //取帧 出队
    struct v4l2_buffer read = {0};
    read.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    read.memory = V4L2_MEMORY_MMAP;
    ret = ioctl(camera_fd_,VIDIOC_DQBUF,&read);
    if(ret < 0)
        perror("读取数据失败");

    //入队
    struct v4l2_buffer qbuf = {0};
    qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    qbuf.memory = V4L2_MEMORY_MMAP;
    qbuf.index = read.index;

    if(ioctl(camera_fd_,VIDIOC_QBUF,&qbuf) < 0)
    {
        perror("qbuf failure");
    }

    *byte_used = read.bytesused;   
    return imx_mpaddr[read.index];
}

void CAMERA::camera_off(void)
{
    //停止采集
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ret = ioctl(camera_fd_,VIDIOC_STREAMOFF,&type);
    if(ret < 0)
        perror("停止采集失败");
    else
        printf("停止采集成功\n");
    
}

void CAMERA::camera_close(void)
{
    //7释放映射
    for (int i = 0; i < max_buffer; i++)
    {
        if(imx_mpaddr[i] != MAP_FAILED)
        ret = munmap(imx_mpaddr[i],imx_buf_length[i]);
        if(ret < 0)
            perror("释放内存映射失败");
    }
    //9关闭设备
    close(camera_fd_);
}


void CAMERA::convertYUYVtoYUV420(uint8_t* yuyv, uint8_t* yuv420, int width, int height) {
    int y_size = width * height;
    uint8_t* y_plane = yuv420;
    uint8_t* u_plane = yuv420 + y_size;
    uint8_t* v_plane = yuv420 + y_size + y_size / 4;
    
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x += 2) {
            int idx = y * width * 2 + x * 2;
            
            // 提取Y分量
            y_plane[y * width + x] = yuyv[idx];
            y_plane[y * width + x + 1] = yuyv[idx + 2];
            
            // 每隔一行采样UV分量（420采样）
            if (y % 2 == 0) {
                int uv_idx = (y / 2) * (width / 2) + (x / 2);
                u_plane[uv_idx] = yuyv[idx + 1];
                v_plane[uv_idx] = yuyv[idx + 3];
            }
        }
    }
}


